#include "WPILib.h"
#include "Joystick.h"
#include "Servo.h"
#include "Relay.h"
#include <Compressor.h>
#include <Solenoid.h>
#include <DriverStationLCD.h>
#include <Timer.h>


class RobotDemo : public SimpleRobot
{
	RobotDrive MyRobot;	//Drive System
	Joystick Controller_1;	//Driver Controller
	Joystick Controller_2;	//Shooter Controller
	Victor Coraller;	//Coraller control	
	Victor Hopper;	//Hopper Control
	Victor Tow_er;	//Tow-er Control
	Victor Shooter_Rotation;	//Shooter Control
	Servo Left_Shifter;	//Left motor super shifter
	Servo Right_Shifter;	//Right motor super shifter
	Solenoid Bolt_Open;	// Piston for shooter reload
	Solenoid Bolt_Closed;	// Piston for shooter reload
	Solenoid Air_Open;	//Fireing mechanism for shooter
	Solenoid Air_Closed;	//Fireing mechanism for shooter
	Solenoid Claw_Open;	// Claw for tow-er
	Solenoid Claw_Closed;	// Claw for tow-er
	Solenoid Shooter_Piston_Open;	//Shooter reload feature
	Solenoid Shooter_Piston_Closed;	//Shooter reload feature
	Solenoid Shooter_Air_Open;	//Shooter air
	Solenoid Shooter_Air_Closed;	//Shooter air
	
	DriverStationLCD *m_LCD;

	
public:
	RobotDemo(void):
		
		MyRobot(1,2),	//Digital Sidecar output 1 & 2
		Controller_1(1),	//USB port 1 in Driver Station
		Controller_2(2),		//USB port 2 in Driver Station
		Coraller(3),	//Digital Sidecar output 3
		Hopper(4),	//Digital Sidecar output 4
		Tow_er(5),	//Digital Sidecar output 5
		Shooter_Rotation(6),	//Digital Sidecar output 6
		Left_Shifter(9),	//Digital Sidecar output 9
		Right_Shifter(10),	//Digital Sidecar output 10
		Bolt_Open(1),	//9472 Module output
		Bolt_Closed(2),	//9472 Module output
		Air_Open(3),	//9472 Module output
		Air_Closed(4),	//9472 Module output
		Claw_Open(5),	//9472 Module output
		Claw_Closed(6),	//9472 Module output
		Shooter_Piston_Open(7),	//9472 Module output
		Shooter_Piston_Closed(8),	//9472 Module output		
		Shooter_Air_Open(2,1),	//9472 Module output
		Shooter_Air_Closed(2,2)	//9472 Module output
		
		
		
	{
		
	}

	/**
	 * Drive left & right motors for 2 seconds then stop
	 */
	void Autonomous(void)
	{
		
	}

	/**
	 * Runs the motors with arcade steering. 
	 */
	void OperatorControl(void)
	{
		
		while (IsOperatorControl())
		{
			//Buttons
				//Driver Controller
				bool Shift_Up;	//Button 6 Shifts gears up
				bool Shift_Down;	//Button 8 shifts gears down
				bool Tow_Activation;	//Buttons 5 activates towing mechanism 
				bool Hopper_Button;	//Button 3 activates hopper
				bool Coraller_Button;	//Button 2 activates coraller
				
				//Shooter Controller
				float Shooter_Aim;	//Turns shooter right
				bool Fire;	//Button 6 fire and actuates the reload sequence
				
			//Actuators Definitions
				//Driver Controller
				Shift_Up = Controller_1.GetRawButton(6);	//Gets state of button 6 on controller 1
				Shift_Down = Controller_1.GetRawButton(8);	//Gets state of button 8 on controller 1
				Tow_Activation = Controller_1.GetRawButton(5);	//Gets state of button 5 on controller 1
				Hopper_Button = Controller_1.GetRawButton(3);	//Gets state of button 3 on controller 1
				Coraller_Button = Controller_1.GetRawButton(2);	//Gets state of button 2 on controller 1
								
				//Shooter Controller
				Shooter_Aim = Controller_2.GetX();	//Gets state of the right button on controller 2
				Fire = Controller_2.GetRawButton(6);	//Gets state of button 6 on controller 2
				
			//Counters and Bools
			bool Coraller_On = false;
			bool Hopper_On = false;
			bool Tow_On = false;
			bool On = true;
			int Counter = 1;
			
			 

			
			//Driver Controller
			
				//Coraller
					if(Coraller_Button && !Coraller_On) 	//If coraller button is pressed and coraller isn't on turn it on
					{
						Coraller_On = true;
						Coraller.Set(1);
						m_LCD->Printf(DriverStationLCD::kUser_Line1, 2,"Coraller was turned on");
					}
					
					else if(Coraller_Button && Coraller_On)	//IF coraller button is pressed and coraller is on turn it off
					{
						Coraller_On = false;
						Coraller.Set(0);
						m_LCD->Printf(DriverStationLCD::kUser_Line1, 2,"Coraller was turned off");
					}
				
				//Hopper
					if(Hopper_Button && !Hopper_On)
					{
						Hopper_On = true;
						Hopper.Set(1);
						m_LCD->Printf(DriverStationLCD::kUser_Line1, 2,"Hopper was deployed");
					}
					else if(Hopper_Button && Hopper_On)
					{
						Hopper_On = false;
						Hopper.Set(0);
						m_LCD->Printf(DriverStationLCD::kUser_Line1, 2,"Hopper was retracted");
					}
					
				
				//Tow-er
					if(Tow_Activation && !Tow_On)
					{
						Claw_Open.Set(On);
						Tow_On = true;
						m_LCD->Printf(DriverStationLCD::kUser_Line1, 2,"Tow-er was deployed");
					}
					else if(Tow_Activation && Tow_On)
					{
						Claw_Closed.Set(On);
						Tow_On = false;
						m_LCD->Printf(DriverStationLCD::kUser_Line1, 2,"Tow-er was retracted");
					}
					
					
				
				//Drive System
					MyRobot.ArcadeDrive(Controller_1);	//Robot arcade drive
					
					if(Shift_Up)
					{
						Right_Shifter.Set(1);
						Left_Shifter.Set(1);
						m_LCD->Printf(DriverStationLCD::kUser_Line1, 2,"You are in high gear");
					}
					else if(Shift_Down)
					{
						Right_Shifter.Set(0);
						Left_Shifter.Set(0);
						m_LCD->Printf(DriverStationLCD::kUser_Line1, 2,"You are in low gear");
					}
					
			
			//Shooter Controller
				
				//Shooter
					if(Shooter_Aim > .5)
					{
						Shooter_Rotation.Set(1);
					}
					else if(Shooter_Aim < .5)
					{
						Shooter_Rotation.Set(-1);
					}
					
					
					if(Fire && Counter == 1)
					{
						Air_Open.Set(On);
						Air_Closed.Set(On);
						Counter == Counter++;
						m_LCD->Printf(DriverStationLCD::kUser_Line1, 2,"First shot fired");
					}
					else if(Fire && Counter > 1)
					{
						m_LCD->Printf(DriverStationLCD::kUser_Line1, 2,"Reloading");
						Shooter_Piston_Open.Set(On);
						Shooter_Piston_Closed.Set(On);
						m_LCD->Printf(DriverStationLCD::kUser_Line1, 2,"Firing!");
						Air_Open.Set(On);
						Air_Closed.Set(On);
					}
					else if(Fire && Counter >= 10)
					{
						m_LCD->Printf(DriverStationLCD::kUser_Line1, 2,"Sorry you are out of ping pong balls to fire");
					}
		}
	}
};

START_ROBOT_CLASS(RobotDemo);

